#include "MephistoCAM.h"

void init_daisy_config( DAISY_Config *cfg ){
    
    cfg->update_adg714      = 1;
    cfg->update_dacx0508_a  = 1;
    cfg->update_dacx0508_b  = 1;
    
    cfg->adg714     = (ADG714_Config*)malloc(sizeof(ADG714_Config));
    cfg->dacx0508_a = (DACx0508_Config*)malloc(sizeof(DACx0508_Config));
    cfg->dacx0508_b = (DACx0508_Config*)malloc(sizeof(DACx0508_Config));
    
    init_adg714_config( cfg->adg714 );
    init_dacx0508_config( cfg->dacx0508_a );
    init_dacx0508_config( cfg->dacx0508_b );
}

void free_daisy_config( DAISY_Config *cfg ){
    free( cfg->adg714 );
    free( cfg->dacx0508_a );
    free( cfg->dacx0508_b );
}

void dump_daisy_config( DAISY_Config *cfg, char *dump_filename ){
    // NOT implemented yet ...
}

void duplicate_daisy_config( DAISY_Config *cfg_old, DAISY_Config *cfg_new ){
    cfg_new->update_adg714      = cfg_old->update_adg714;
    cfg_new->update_dacx0508_a  = cfg_old->update_dacx0508_a;
    cfg_new->update_dacx0508_b  = cfg_old->update_dacx0508_b;

    duplicate_ADG714( cfg_old->adg714, cfg_new->adg714 );

    duplicate_DACx0508( cfg_old->dacx0508_a, cfg_new->dacx0508_a );

    duplicate_DACx0508( cfg_old->dacx0508_b, cfg_new->dacx0508_b );

    sprintf(cfg_new->dump_dir, "%s", cfg_old->dump_dir);
}

int read_daisy_config( DAISY_Config *cfg, char *inifile ){

    dictionary *ini = iniparser_load( inifile );
    cfg->update_adg714      = iniparser_getint( ini, "ADG714:update_adg714", 1 );
    cfg->update_dacx0508_a  = iniparser_getint( ini, "ADG714:update_dacx0508_a", 1 );
    cfg->update_dacx0508_b  = iniparser_getint( ini, "ADG714:update_dacx0508_b", 1 );

    iniparser_freedict( ini );

    read_adg714_config( cfg->adg714, inifile, "ADG714" );
    read_dacx0508_config( cfg->dacx0508_a, inifile, "DACx0508_A" );
    read_dacx0508_config( cfg->dacx0508_b, inifile, "DACx0508_B" );

    uint32_t steps_a = cfg->dacx0508_a->steps;
    uint32_t steps_b = cfg->dacx0508_b->steps;

    uint32_t sleep_time_us_a = cfg->dacx0508_a->sleep_time_us;
    uint32_t sleep_time_us_b = cfg->dacx0508_a->sleep_time_us;

    uint32_t steps = (steps_a > steps_b) ? steps_a : steps_b;
    uint32_t sleep_time_us = (sleep_time_us_a > sleep_time_us_b) ? sleep_time_us_a : sleep_time_us_b;

    cfg->dacx0508_a->steps = steps;
    cfg->dacx0508_b->steps = steps;

    cfg->dacx0508_a->sleep_time_us = sleep_time_us;
    cfg->dacx0508_b->sleep_time_us = sleep_time_us;

    return 0;
}

int set_reg_update_option(  DAISY_Config *cfg, uint32_t *gpio_reg ){

    if( cfg->update_adg714 == 1 )
        setbit( *gpio_reg, BIT_UPDATE_SPI_DATA1 );
    else
        clrbit( *gpio_reg, BIT_UPDATE_SPI_DATA1 );

    if( cfg->update_dacx0508_a == 1 )
        setbit( *gpio_reg, BIT_UPDATE_SPI_DATA2 );
    else
        clrbit( *gpio_reg, BIT_UPDATE_SPI_DATA2 );
    
    if( cfg->update_dacx0508_b == 1 )
        setbit( *gpio_reg, BIT_UPDATE_SPI_DATA3 );
    else
        clrbit( *gpio_reg, BIT_UPDATE_SPI_DATA3 );

    return 0;
}

/*
void get_reg_data( 	DAISY_Config *cfg, 
					uint32_t *reg_adg714,
					uint32_t *reg_dacx0508_a,
					uint32_t *reg_dacx0508_b,
					int reg_idx, // apply to DACx0508 only
					int state,
					double scale
					){


}
*/

//  print daisy configurations for debug
int print_daisy_config( DAISY_Config *cfg ){

    printf("\n\n###################### DAISY CHAIN ####################\n");
    
    print_adg714_config( cfg->adg714 );
    print_dacx0508_config( cfg->dacx0508_a );
    print_dacx0508_config( cfg->dacx0508_b );
    
    printf("###################### DAISY CHAIN ####################\n\n");
    
    return 0;
}


int config_daisy_via_SPI(   uint32_t *gpio_reg, 
					        uint32_t *gpio2_reg, 
					        uint32_t *gpio_in_reg,
					        SPI_CONFIG *spi_config,
					        DAISY_Config *cfg,
                            uint32_t state,
                            int verbose
					        ){


    int nsteps_a = cfg->dacx0508_a->steps;
    int nsteps_b = cfg->dacx0508_b->steps;
    int nsteps = nsteps_a > nsteps_b ? nsteps_a : nsteps_b;

    int reg_num = cfg->dacx0508_a->reg_num;
	int idx;
    int step;
    
    double *scale = (double*)malloc(sizeof(double)*nsteps);

    if( state == 0 ){
        int i;
        for(i=0; i<nsteps; i++){
            scale[i] = 1.0 - (i+1.0) / nsteps;
        }
    } else if( state == 1 ){
        int i;
        for(i=0; i<nsteps; i++){
            scale[i] = (i+1.0) / nsteps;
        }
    }

	int used_reg_num = reg_num;
    int daisychain_size = 3;                                    // 3 = ADG714 + 2 * DACx0508

    SPI_DATA *spi_data = (SPI_DATA*)malloc(sizeof(SPI_DATA));   // prepare SPI data to be transferred
    spi_data_alloc(spi_data, used_reg_num*daisychain_size);                   
    
    uint32_t sleep_time_us_a = cfg->dacx0508_a->sleep_time_us;
    uint32_t sleep_time_us_b = cfg->dacx0508_b->sleep_time_us;
    uint32_t sleep_time_us = (sleep_time_us_a > sleep_time_us_b) ? sleep_time_us_a : sleep_time_us_a;
    
    if( verbose == 1 ){
        if( state == 0 )
            printf("> (daisy) powering down ... \n");
        else
            printf("> (daisy) powering up ... \n");
    }

    for( step=0; step<nsteps; step++ ){

        if( verbose == 1 ){
            printf("> (daisy) --> %g%%\n", 100*scale[step]);
        }

        int spi_data_idx = 0;
        
        for( idx=0; idx<reg_num; idx+=1 ){
        
            uint32_t addr_adg714        = 0;    // 对ADG714而言，这个地址只是“占位”而已
            uint32_t addr_dacx0508_a    = cfg->dacx0508_a->reg_addr[idx];
            uint32_t addr_dacx0508_b    = cfg->dacx0508_b->reg_addr[idx];
            
            uint32_t data_adg714        = get_adg714_reg_data( cfg->adg714 );
            uint32_t data_dacx0508_a    = cfg->dacx0508_a->reg_data[idx];
            uint32_t data_dacx0508_b    = cfg->dacx0508_b->reg_data[idx];

            // 对发送给DACx0508的数据进行re-scale
            if( idx >= 2 ){
            
                int data_a = data_dacx0508_a;
                int data_b = data_dacx0508_b;
                
                data_a = (int)(data_a*scale[step]);
                data_b = (int)(data_b*scale[step]);

                data_dacx0508_a = (uint32_t)data_a;
                data_dacx0508_b = (uint32_t)data_b;

                // printf("> (daisy) data_dacx0508_a = %6d\tdata_dacx0508_b = %6d\n",
                        // data_dacx0508_a, data_dacx0508_b);
            }

        //  send data to registers on PL-side
            spi_data_set_LS( spi_data, 
                             spi_data_idx*daisychain_size+0, 
                             addr_adg714, 
                             data_adg714,
                             0 );  // left-shifted 0bits

            spi_data_set_LS( spi_data, 
                             spi_data_idx*daisychain_size+1, 
                             addr_dacx0508_a, 
                             data_dacx0508_a,
                             16 );  // left-shifted 16bits
                             
            spi_data_set_LS( spi_data, 
                             spi_data_idx*daisychain_size+2, 
                             addr_dacx0508_b, 
                             data_dacx0508_b,
                             16 );  // left-shifted 16bits

            spi_data_idx++;              
        }
        
        // send SPI data to DACx0508
        send_spi_data_daisy_chain_mode( gpio_reg,
                                        gpio2_reg,
                                        gpio_in_reg,
                                        spi_config, 
                                        spi_data );

        usleep(sleep_time_us);
    }

    if( verbose == 1 ){
        if( state == 0 )
            printf("> (daisy) powering down done! \n");
        else
            printf("> (daisy) powering up done! \n");
    }

    free(scale);

    spi_data_free(spi_data);	
    
    return 0;				        
}


// 将VOD从VOD_MIN恢复至29V
int restore_OD_bias_voltage(    uint32_t *gpio_reg, 
                                uint32_t *gpio2_reg, 
                                uint32_t *gpio_in_reg,
                                SPI_CONFIG *spi_config,
                                DAISY_Config *cfg
                             ){

    DAISY_Config *cfg_new = (DAISY_Config*)malloc(sizeof(DAISY_Config));
    init_daisy_config( cfg_new );
    duplicate_daisy_config( cfg, cfg_new );

    cfg_new->update_adg714     = 0;
    cfg_new->update_dacx0508_a = 0;
    cfg_new->update_dacx0508_b = 1;
    cfg_new->dacx0508_a->sleep_time_us = 1000; // 1ms
    cfg_new->dacx0508_b->sleep_time_us = 1000; // 1ms
    cfg_new->dacx0508_a->steps = 1;
    cfg_new->dacx0508_b->steps = 1;

    int i;

    // update vout
    for( i=0; i<8; i++ ){
        cfg_new->dacx0508_b->reg_data[i+2] = get_dac_code( 29.0, cfg_new->dacx0508_b->gain[i]);
        // cfg_new->dacx0508_b->reg_data[i+2] = get_dac_code( 25.0, cfg_new->dacx0508_b->gain[i]);
    }

    config_daisy_via_SPI( gpio_reg, 
				          gpio2_reg, 
				          gpio_in_reg,
				          spi_config,
				          cfg_new,
                          1,
                          0
				        );

    usleep(50000); // 50ms

    free_daisy_config(cfg_new);

    return 0;
}

// 将VOD从29V降至VOD_MIN
int lower_OD_bias_voltage(  uint32_t *gpio_reg, 
                            uint32_t *gpio2_reg, 
                            uint32_t *gpio_in_reg,
                            SPI_CONFIG *spi_config,
                            DAISY_Config *cfg
                            ){

    DAISY_Config *cfg_new = (DAISY_Config*)malloc(sizeof(DAISY_Config));
    init_daisy_config( cfg_new );
    duplicate_daisy_config( cfg, cfg_new );

    cfg_new->update_adg714     = 0;
    cfg_new->update_dacx0508_a = 0;
    cfg_new->update_dacx0508_b = 1;
    cfg_new->dacx0508_a->sleep_time_us = 1000; // 1ms
    cfg_new->dacx0508_b->sleep_time_us = 1000; // 1ms
    cfg_new->dacx0508_a->steps = 1;
    cfg_new->dacx0508_b->steps = 1;

    // update vout
    int i;
    for( i=0; i<8; i++ ){
        cfg_new->dacx0508_b->reg_data[i+2] = get_dac_code( _VOD_MIN_, cfg_new->dacx0508_b->gain[i]);
    }

    config_daisy_via_SPI( gpio_reg, 
				          gpio2_reg, 
				          gpio_in_reg,
				          spi_config,
				          cfg_new,
                          1,
                          0
				        );

    free_daisy_config(cfg_new);

    return 0;
}

// // the following two are the same as the above two, but are to be used inside socket service.
// int restore_OD_bias_voltage_socket(){

// }

// int lower_OD_bias_voltage_socket(){

// }

